/* -*- Mode:C++; c-file-style:"gnu"; indent-tabs-mode:nil; -*- */
/*
 * Copyright (c) 2019 HUST Dian Group
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation;
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * Original Author: Pengyu Liu <eicliupengyu@gmail.com>
 */
#include <cmath>
#include <algorithm>
#include "ns3/log.h"
#include "ns3/simulator.h"
#include "robo-actor.h"

namespace ns3 {
NS_LOG_COMPONENT_DEFINE ("RoboActor");
NS_OBJECT_ENSURE_REGISTERED (RoboActor);
RoboActor::RoboActor (void)
{
  NS_LOG_FUNCTION (this);
}

RoboActor::~RoboActor (void)
{
  NS_LOG_FUNCTION (this);
}

TypeId
RoboActor::GetTypeId (void)
{
  static TypeId tid = TypeId ("ns3::RoboActor")
                          .SetParent<Object> ()
                          .SetGroupName ("Robo")
                          .AddConstructor<RoboActor> ();
  return tid;
}

TypeId
RoboActor::GetInstanceTypeId (void) const
{
  return GetTypeId ();
}

void
RoboActor::DoDispose (void)
{
  m_mobility = NULL;
}

void
RoboActor::SetCollisionType (CollisionType type)
{
  m_collisionType = type;
}

CollisionType
RoboActor::GetCollisionType (void) const
{
  return m_collisionType;
}

void
RoboActor::SetGlobalLocation (FVector globallocation)
{
  m_globalLocation = globallocation;
}

FVector
RoboActor::GetGlobalLocation (void) const
{
  return m_globalLocation;
}

void
RoboActor::SetGlobalRotation (FAngle globalrotation)
{
  m_globalRotation = globalrotation;
}

FAngle
RoboActor::GetGlobalRotation (void) const
{
  return m_globalRotation;
}

void
RoboActor::SetCollisionMask (uint32_t collisionMask)
{
  m_collisionMask = collisionMask;
}

uint32_t
RoboActor::GetCollisionMask (void) const
{
  return m_collisionMask;
}

uint32_t
RoboActor::GetSelfMask (void) const
{
  return m_selfMask;
}

void
RoboActor::SetSelfMask (uint32_t selfMask)
{
  m_selfMask = selfMask;
}

void
RoboActor::SetRedius (float redius)
{
  NS_ASSERT_MSG (redius > 0.f, "The redius of a circle must be bigger than 0.");
  m_redius = redius;
}

float
RoboActor::GetRedius (void) const
{
  return m_redius;
}

void
RoboActor::AddBoundaryPoint (FVector boundaryPoint)
{
  m_boundaryPoint.push_back (boundaryPoint);
  m_redius = fmax (boundaryPoint.GetLength (), m_redius);
}

void
RoboActor::AddBoundaryPoint (std::vector<FVector> boundaryPoints)
{
  FVector basePoint = boundaryPoints[0];
  std::sort (boundaryPoints.begin () + 1, boundaryPoints.end (), [basePoint](FVector x, FVector y) {
    float cross = (x - basePoint) * (y - basePoint);
    return cross > 0 || (cross == 0 && x.GetDistance2 (basePoint) < y.GetDistance2 (basePoint));
  });
  for (auto &each : boundaryPoints)
    {
      AddBoundaryPoint (each);
    }
}

bool
IsCollision (Ptr<RoboActor> obj1, Ptr<RoboActor> obj2)
{
  if (!(obj1->m_selfMask & obj2->m_collisionMask))
    {
      return false;
    }
  if (obj1->m_collisionType == Collision_Type_Boundary)
    {
      return IsObjectBoundaryCollision (obj2, obj1);
    }
  if (obj2->m_collisionType == Collision_Type_Boundary)
    {
      return IsObjectBoundaryCollision (obj1, obj2);
    }
  if (!(obj1->m_globalLocation.GetDistance (obj2->m_globalLocation) <=
        (obj1->m_redius + obj2->m_redius)))
    {
      return false;
    }
  if (obj1->m_collisionType == Collision_Type_Circle &&
      obj2->m_collisionType == Collision_Type_Circle)
    {
      return true;
    }
  if (obj1->m_collisionType == Collision_Type_Circle &&
      obj2->m_collisionType == Collision_Type_Polygon)
    {
      return IsCirclePolygonCollision (obj1, obj2);
    }
  if (obj2->m_collisionType == Collision_Type_Circle &&
      obj1->m_collisionType == Collision_Type_Polygon)
    {
      return IsCirclePolygonCollision (obj2, obj1);
    }

  if (obj1->m_collisionType == Collision_Type_Polygon &&
      obj2->m_collisionType == Collision_Type_Polygon)
    {
      return IsPolygonPolygonCollision (obj1, obj2);
    }
  return false;
}

bool
IsPolygonPolygonCollision (Ptr<RoboActor> obj1, Ptr<RoboActor> obj2)
{
  std::vector<FVector> globalBound1;
  globalBound1.clear ();
  for (auto &i : obj1->m_boundaryPoint)
    {
      globalBound1.push_back (ConvCoordinate (obj1->m_globalLocation, obj1->m_globalRotation, i));
    }
  std::vector<FVector> globalBound2;
  globalBound2.clear ();
  for (auto &i : obj2->m_boundaryPoint)
    {
      globalBound2.push_back (ConvCoordinate (obj2->m_globalLocation, obj2->m_globalRotation, i));
    }
  auto len1 = globalBound1.size ();
  auto len2 = globalBound2.size ();
  NS_ASSERT_MSG (len1 > 1 && len2 > 1, "Boundary is empty");
  if (len1 == 2 && len2 == 2)
    {
      return IsCross (globalBound1[0], globalBound1[1], globalBound2[0], globalBound2[1]);
    }
  if (len1 == 2)
    {
      for (uint32_t i = 1; i < len2; ++i)
        {
          if (IsCross (globalBound1[0], globalBound1[1], globalBound2[i - 1], globalBound2[i]))
            {
              return true;
            }
        }
      if (IsCross (globalBound1[0], globalBound1[1], globalBound2[len2 - 1], globalBound2[0]))
        {
          return true;
        }
      for (uint32_t i = 2; i < len2; ++i)
        {
          if (PointInTriangle (globalBound1[0], globalBound2[0], globalBound2[i - 1],
                               globalBound2[i]))
            {
              return true;
            }
        }
      return false;
    }
  if (len2 == 2)
    {
      for (uint32_t i = 1; i < len1; ++i)
        {
          if (IsCross (globalBound2[0], globalBound2[1], globalBound1[i - 1], globalBound1[i]))
            {
              return true;
            }
        }
      if (IsCross (globalBound2[0], globalBound2[1], globalBound1[len1 - 1], globalBound1[0]))
        {
          return true;
        }
      for (uint32_t i = 2; i < len1; ++i)
        {
          if (PointInTriangle (globalBound2[0], globalBound1[0], globalBound1[i - 1],
                               globalBound1[i]))
            {
              return true;
            }
        }
      return false;
    }
  for (uint32_t i = 0; i < len1; ++i)
    {
      for (uint32_t j = 0; j < len2; ++j)
        {
          if (IsCross (globalBound1[(i < 1 ? len1 : i) - 1], globalBound1[i],
                       globalBound2[(j < 1 ? len2 : j) - 1], globalBound2[j]))
            {
              return true;
            }
        }
    }
  for (uint32_t i = 2; i < len1; ++i)
    {
      for (uint32_t j = 0; j < len2; ++j)
        {
          if (PointInTriangle (globalBound2[j], globalBound1[0], globalBound1[i - 1],
                               globalBound1[i]))
            {
              return true;
            }
        }
    }
  return false;
}

bool
IsCirclePolygonCollision (Ptr<RoboActor> obj1, Ptr<RoboActor> obj2)
{
  std::vector<FVector> globalBound;
  globalBound.clear ();
  for (auto &i : obj2->m_boundaryPoint)
    {
      globalBound.push_back (ConvCoordinate (obj2->m_globalLocation, obj2->m_globalRotation, i));
    }
  auto len = globalBound.size ();
  for (uint32_t i = 1; i < len; ++i)
    {
      if (CircleCrossLineSegment (obj1->m_globalLocation, obj1->m_redius, globalBound[i - 1],
                                  globalBound[i]))
        {
          return true;
        }
    }
  if (CircleCrossLineSegment (obj1->m_globalLocation, obj1->m_redius, globalBound[len - 1],
                              globalBound[0]))
    {
      return true;
    }
  for (uint32_t i = 2; i < len; ++i)
    {
      if (PointInTriangle (obj1->m_globalLocation, globalBound[0], globalBound[i - 1],
                           globalBound[i]))
        {
          return true;
        }
    }
  return false;
}

bool
IsObjectBoundaryCollision (Ptr<RoboActor> obj1, Ptr<RoboActor> obj2)
{
  return !(obj1->m_globalLocation.m_x > obj2->m_boundaryPoint[0].m_x &&
           obj1->m_globalLocation.m_y > obj2->m_boundaryPoint[0].m_y &&
           obj1->m_globalLocation.m_x < obj2->m_boundaryPoint[1].m_x &&
           obj1->m_globalLocation.m_y < obj2->m_boundaryPoint[1].m_y);
}

void
RoboActor::IndicateCollision (Ptr<RoboActor> oth)
{
  if (m_isDeleted)
    {
      return;
    }
  NS_LOG_FUNCTION (this << static_cast<double> (Simulator::Now ().GetSeconds ()));
  for (auto &cb : m_collisionCallbackList)
    {
      if (!cb.IsNull ())
        {
          cb (oth);
        }
    }
}

uint32_t
RoboActor::AddCollisionCallback (CollisionCallback cb)
{
  NS_LOG_FUNCTION (this);
  m_collisionCallbackList.push_back (cb);
  return m_collisionCallbackList.size () - 1;
}

uint32_t
RoboActor::AddUpdateCallback (UpdateCallback cb)
{
  NS_LOG_FUNCTION (this);
  m_updateCallbackList.push_back (cb);
  return m_updateCallbackList.size () - 1;
}

void
RoboActor::Update (float delta)
{
  NS_LOG_FUNCTION (this);
  if (m_isDestroy || m_isDeleted)
    {
      return;
    }
  for (auto &cb : m_updateCallbackList)
    {
      if (!cb.IsNull ())
        {
          cb (delta);
        }
    }
}

void
RoboActor::SetMobility (Ptr<RoboMobility> mob)
{
  m_mobility = mob;
}

Ptr<RoboMobility>
RoboActor::GetMobility ()
{
  return m_mobility;
}

} // namespace ns3
